{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Basics of Automatic Differentiation\n",
    "For instructions on how to run these tutorial notebooks, please see the [index](./index.ipynb).\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook provides a short tutorial for automatic differentiation in Drake. It covers the automatic differentiation of\n",
    "- Basic Eigen/Numpy operations,\n",
    "- Dynamical system operations (state update, output calculation, etc.),\n",
    "- Sampled data from simulation rollout."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c5702596-346e-457e-9e21-677699f0860e",
   "metadata": {},
   "source": [
    "### Eigen/Numpy Operations\n",
    "\n",
    "Drake uses Eigen's AutoDiffScalar for automatic differentiation. Any explicit Eigen (and hence Numpy in Python) operations can be automatically differentiated. Let's consider the simple example $\\mathbf{y} = \\mathbf{a}^{\\top} \\mathbf{x} = [1,~ 2] \\mathbf{x}$. We want to obtain the derivative $\\partial \\mathbf{y} / \\partial \\mathbf{x} = \\mathbf{a}^{\\top}$ using automatic differentiation. Here is how to do it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "93615a55-c7ba-45be-b48b-c1a488e59ea9",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.autodiffutils import InitializeAutoDiff, ExtractGradient\n",
    "\n",
    "a = np.array([1, 2]).reshape([2, -1])\n",
    "x = np.random.rand(2, 1)\n",
    "print(\"double type array:\\n\", x)\n",
    "x = InitializeAutoDiff(x)\n",
    "print(\"converted to AutoDiffXd scalar type array:\\n\", x)\n",
    "y = a.T @ x\n",
    "dydx = ExtractGradient(y)\n",
    "print(\"Gradient:\", ExtractGradient(y))\n",
    "np.testing.assert_allclose(a.T, dydx)  # assert dy/dx = a^T"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9935ddb0-42ee-40a3-9495-f73ff7fd3b53",
   "metadata": {},
   "source": [
    "Note that for automatic differentiation, we just need two extra steps in addition to the usual explicit Numpy operations: 1) Declare the variable with respect to which we want to differentiate using `InitializeAutoDiff(x)`, and 2) extract the gradient in the end using `ExtractGradient(y)`.\n",
    "\n",
    "We can also differentiate w.r.t. multiple variables. Consider the example $\\mathbf{y} = \\mathbf{a}_1^{\\top} \\mathbf{x}_1 + \\mathbf{a}_2^{\\top} \\mathbf{x}_2$, where we want to obtain $\\partial \\mathbf{y} / \\partial \\mathbf{x}_1$ and $\\partial \\mathbf{y} / \\partial \\mathbf{x}_2$. Here is how to do it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d34153f7-4ddd-42bb-a73d-ae28db7a9db0",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.autodiffutils import InitializeAutoDiffTuple, ExtractGradient\n",
    "\n",
    "a1 = np.array([1, 2]).reshape([2, -1])\n",
    "x1 = np.random.rand(2, 1)\n",
    "\n",
    "a2 = np.array([1, 2, 3]).reshape([3, -1])\n",
    "x2 = np.random.rand(3, 1)\n",
    "\n",
    "x1, x2 = InitializeAutoDiffTuple(x1, x2)\n",
    "y = a1.T @ x1+ a2.T @ x2\n",
    "\n",
    "dydx = ExtractGradient(y)\n",
    "print(\"All gradients:\", dydx)\n",
    "dydx1 = dydx[0][0:2]\n",
    "dydx2 = dydx[0][2:]\n",
    "print(\"Gradients calculated by automatic differentiation:\")\n",
    "print(\"a1^T =\", dydx1)\n",
    "np.testing.assert_allclose(a1.T, [dydx1])\n",
    "print(\"a2^T =\", dydx2)\n",
    "np.testing.assert_allclose(a2.T, [dydx2])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9121462a-fdbd-46d4-b6ab-8f9e6b0b1af7",
   "metadata": {},
   "source": [
    "Note that `ExtractGradient(y)` extracts derivatives of `y` with respect to _all_ variables declared by `InitializeAutodiffTuple`."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a84482b6-ebe1-4236-942d-2d64bd3c713f",
   "metadata": {},
   "source": [
    "## Dynamical Systems"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0f40ceb8-5be5-432d-99dc-2930313b1755",
   "metadata": {},
   "source": [
    "### Drake's Built-in Dynamical Systems\n",
    "\n",
    "Drake's most built-in systems' dynamics only involve explicit Eigen operations. Hence, they are all automatically differentiable. Let's consider the simple discrete-time [LinearSystem](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_linear_system.html), whose dynamics is given as\n",
    "$$x_{t+1} = x_t +  2u_t, ~~~y_{t} = 3x_t + 4u_t.$$\n",
    "For general dynamical systems, the derivatives of next state w.r.t. state $\\partial x_{t+1}/ \\partial x_t$ and input $\\partial x_{t+1}/ \\partial u_t$, and output w.r.t. state $\\partial y_{t}/ \\partial x_t$ and input $\\partial y_{t}/ \\partial u_t$ are frequently wanted. Here we will show how to obtain them via automatic differentiation. Let's first construct the system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "00923cbf-1377-40ab-911b-7746cf2296ba",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.systems.primitives import LinearSystem\n",
    "from pydrake.autodiffutils import InitializeAutoDiffTuple, ExtractGradient\n",
    "\n",
    "A = np.array([[1]])\n",
    "B = np.array([[2]])\n",
    "C = np.array([[3]])\n",
    "D = np.array([[4]])\n",
    "timestep = 1  # so that the system is discrete-time\n",
    "system = LinearSystem(A, B, C, D, timestep)\n",
    "\n",
    "print(\"A system using double:\", system)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dd8cf085-b641-4cd9-9b15-ef29f58407c5",
   "metadata": {},
   "source": [
    "By default, the system uses `double` as the scalar type. We need to convert it to use [drake::AutoDiffXd](https://drake.mit.edu/doxygen_cxx/namespacedrake.html#a35725b277b02aeb79f24fd7f724e6dbc):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f82d93fb-9793-43c4-8c42-741051e187fc",
   "metadata": {},
   "outputs": [],
   "source": [
    "system_ad = system.ToAutoDiffXd()\n",
    "print(\"The system converted to AutoDiffXd:\", system_ad)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f48f9342-4116-4ce4-8cc7-8272f7c26ab1",
   "metadata": {},
   "source": [
    "Let's set $x_t = 1$ and $u_t=1$ (or any real numbers)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3d62f16e-4cd0-4465-92f3-6c267ca2a82e",
   "metadata": {},
   "outputs": [],
   "source": [
    "context_ad = system_ad.CreateDefaultContext()\n",
    "x = np.array([1])\n",
    "u = np.array([1])\n",
    "x, u = InitializeAutoDiffTuple(x, u)\n",
    "context_ad.SetDiscreteState(0, x)\n",
    "system_ad.get_input_port(0).FixValue(context_ad, u)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "044dd735-33c6-457d-ba16-aced9d27df9e",
   "metadata": {},
   "source": [
    "Then, we calculate the derivatives of next states $\\partial x_{t+1}/ \\partial x_t=1$ and $\\partial x_{t+1}/ \\partial u_t=2$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8e2f054-2bf6-4cc9-b3fe-3eb56f4a6a96",
   "metadata": {},
   "outputs": [],
   "source": [
    "# allocate the state object\n",
    "x_next_object = system_ad.AllocateContext().get_discrete_state()  \n",
    "# store value to x_next_object without modifying context\n",
    "system_ad.CalcForcedDiscreteVariableUpdate(context_ad, x_next_object)  \n",
    "# to extract numpy array from the state object\n",
    "x_next = x_next_object.get_vector(0).CopyToVector()  \n",
    "grad = ExtractGradient(x_next)\n",
    "dx_next_dx = grad.flatten()[0]\n",
    "dx_next_du = grad.flatten()[1]\n",
    "\n",
    "print(\"Gradients calculated by automatic differentiation:\")\n",
    "print(\"dx'/dx =\", dx_next_dx)\n",
    "assert dx_next_dx == 1\n",
    "print(\"dx'/du =\", dx_next_du)\n",
    "assert dx_next_du == 2"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b5f8a3c0-8e40-454b-80db-a1abc59acf62",
   "metadata": {},
   "source": [
    "and the derivatives of output $\\partial y_{t}/ \\partial x_t=3$ and $\\partial x_{t}/ \\partial u_t=4$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "01a784b8-f212-41da-9137-0a77c540b60e",
   "metadata": {},
   "outputs": [],
   "source": [
    "output_object = system_ad.AllocateOutput()\n",
    "system_ad.CalcOutput(context_ad, output_object)\n",
    "output_port_index = system_ad.get_output_port(0).get_index()\n",
    "output = output_object.get_vector_data(output_port_index).CopyToVector()\n",
    "grad = ExtractGradient(output)\n",
    "dy_dx = grad.flatten()[0]\n",
    "dy_du = grad.flatten()[1]\n",
    "print(\"Gradients calculated by automatic differentiation::\")\n",
    "print(\"dy/dx =\", dy_dx)\n",
    "assert dy_dx == 3\n",
    "print(\"dy/du =\", dy_du)\n",
    "assert dy_du == 4"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6cd828b8-3f19-4aa3-a904-93ee197a465a",
   "metadata": {},
   "source": [
    "### Write Your Own Dynamical Systems for Automatic Differentiation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0e7391a0-c8af-44a3-8ede-cb9d434bb254",
   "metadata": {},
   "source": [
    "You can write your own [LeafSystem](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_leaf_system.html) that supports automatic differentiation by using the [drake::AutoDiffXd](https://drake.mit.edu/doxygen_cxx/namespacedrake.html#a35725b277b02aeb79f24fd7f724e6dbc) scalar type as the template value. In Python, you can do so using the [TemplateSystem](https://drake.mit.edu/pydrake/pydrake.systems.scalar_conversion.html) utility. Let's consider the simple discrete-time system \n",
    "$$x_{t+1} = x_t +  2u_t, ~~~y_{t} = x^2_t.$$\n",
    "We will build it with a discrete-time [LinearSystem](https://drake.mit.edu/doxygen_cxx/classdrake_1_1systems_1_1_linear_system.html), and a custom system that squares the input as the output. Let's first define the linear system $x_{t+1} = x_t +  2u_t, ~~~y_{t} = x_t$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "407eb319-88a5-4e43-8999-78b5262ad16e",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.systems.scalar_conversion import TemplateSystem\n",
    "from pydrake.systems.primitives import LinearSystem\n",
    "from pydrake.systems.framework import (\n",
    "    BasicVector_,\n",
    "    DiagramBuilder,\n",
    "    LeafSystem_,\n",
    ")\n",
    "from pydrake.autodiffutils import InitializeAutoDiffTuple, ExtractGradient\n",
    "\n",
    "A = np.array([[1]])\n",
    "B = np.array([[2]])\n",
    "C = np.array([[1]])\n",
    "D = np.array([[0]])\n",
    "timestep = 1\n",
    "linear_system = LinearSystem(A, B, C, D, timestep)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "aaadc412-1cb4-4740-a70e-1f182f346358",
   "metadata": {},
   "source": [
    "Now, let's define the templated custom system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f75d8340-9496-49d9-b4fd-7db8297e72bf",
   "metadata": {},
   "outputs": [],
   "source": [
    "@TemplateSystem.define(\"SquareSystem_\")\n",
    "def SquareSystem_(T):\n",
    "    class Impl(LeafSystem_[T]):\n",
    "        def _construct(self, dimension: int, converter=None):\n",
    "            LeafSystem_[T].__init__(self, converter=converter)\n",
    "            self.dimension = dimension\n",
    "            self.input_port = self.DeclareVectorInputPort(\n",
    "                \"input\", BasicVector_[T](dimension)\n",
    "            )\n",
    "            self.output_port = self.DeclareVectorOutputPort(\n",
    "                \"output\",\n",
    "                BasicVector_[T](dimension),\n",
    "                self.calc_output,\n",
    "            )\n",
    "\n",
    "        def _construct_copy(self, other, converter=None):\n",
    "            Impl._construct(self, other.dimension, converter=converter)\n",
    "\n",
    "        def calc_output(self, context, output):\n",
    "            input_array = self.input_port.Eval(context)\n",
    "            # Element-wise squared input as the output y = x * x\n",
    "            output.set_value(input_array * input_array)\n",
    "\n",
    "    return Impl\n",
    "\n",
    "SquareSystem = SquareSystem_[None]  # The default system that uses double as the scalar"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3bbd557a-2434-4bba-936b-3785fe7235d5",
   "metadata": {},
   "source": [
    "The main difference from what we saw in [Modeling Dynamical Systems](./dynamical_systems.ipynb) is the use of template classes `LeafSystem_[T]` and `BasicVector_[T]`. The default `double`-scalar class, defined as `SquareSystem = SquareSystem_[None]`, is nearly identical to a version defined without using template classes. However, by templating the system, Drake can automatically convert it to use [drake::AutoDiffXd](https://drake.mit.edu/doxygen_cxx/namespacedrake.html#a35725b277b02aeb79f24fd7f724e6dbc) for automatic differentiation. Now let’s construct the custom system and compose the full diagram:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "18b1aedc-0f1a-4b93-896c-0a11ca4812b1",
   "metadata": {},
   "outputs": [],
   "source": [
    "squared_output = SquareSystem(dimension=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5da8ed62-0f43-4e60-8ed1-3c97bd114494",
   "metadata": {},
   "outputs": [],
   "source": [
    "builder = DiagramBuilder()\n",
    "builder.AddSystem(linear_system)\n",
    "builder.AddSystem(squared_output)\n",
    "builder.Connect(linear_system.get_output_port(0), squared_output.get_input_port(0))\n",
    "builder.ExportInput(linear_system.get_input_port(), \"input\")\n",
    "builder.ExportOutput(squared_output.get_output_port(), \"output\")\n",
    "# The full dynamical system we are considering\n",
    "system = builder.Build()  \n",
    "print(\"Default double systems:\\n\", system.GetSystems())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "31d84c19-fa22-468a-95bc-88a284a8e0b6",
   "metadata": {},
   "source": [
    "Note that although we only construct the default `SquareSystem` that uses `double` as the scalar, it can be converted into a system using [AutoDiffXd](https://drake.mit.edu/doxygen_cxx/namespacedrake.html#a35725b277b02aeb79f24fd7f724e6dbc) as the scalar when we do `ToAutoDiffXd()`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ddfafe53-bef2-4181-a9ff-c2dfaa92ad0a",
   "metadata": {},
   "outputs": [],
   "source": [
    "system_ad = system.ToAutoDiffXd()\n",
    "print(\"AutoDiffXd systems:\\n\", system_ad.GetSystems())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23f8535a-e053-407a-8186-e984ac3d9372",
   "metadata": {},
   "source": [
    "Now let's calculate the derivatives at $x_t=1$ and $u_t=1$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5e7913fd-e2c4-4035-8502-9a3984012ddc",
   "metadata": {},
   "outputs": [],
   "source": [
    "context_ad = system_ad.CreateDefaultContext()\n",
    "x = np.array([1])\n",
    "u = np.array([1])\n",
    "x, u = InitializeAutoDiffTuple(x, u)\n",
    "context_ad.SetDiscreteState(0, x)\n",
    "system_ad.get_input_port(0).FixValue(context_ad, u)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0174abf6-6382-41b6-a785-c0d962c5da39",
   "metadata": {},
   "source": [
    "We obtain the correct derivatives $\\left. \\partial y_{t}/ \\partial x_t \\right|_{x_t=1}= \\left. 2 x_t \\right|_{x_t=1} = 2$ and $\\partial y_{t}/ \\partial u_t=0$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a36c8a8f-15c7-4d36-8047-1843853eb473",
   "metadata": {},
   "outputs": [],
   "source": [
    "output_object = system_ad.AllocateOutput()\n",
    "system_ad.CalcOutput(context_ad, output_object)\n",
    "output_port_index = system_ad.GetOutputPort(\"output\").get_index()\n",
    "output = output_object.get_vector_data(output_port_index).CopyToVector()\n",
    "grad = ExtractGradient(output)\n",
    "dy_dx = grad.flatten()[0]\n",
    "dy_du = grad.flatten()[1]\n",
    "print(\"Gradients calculated by automatic differentiation:\")\n",
    "print(\"dy/dx =\", dy_dx)\n",
    "assert dy_dx == 2\n",
    "print(\"dy/du =\", dy_du)\n",
    "assert dy_du == 0"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c3e87473-b6fd-4d39-880e-4a7b0c9df408",
   "metadata": {},
   "source": [
    "## Simulation Rollouts"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7e5d4620-eb45-4800-954b-36f6a354908b",
   "metadata": {},
   "source": [
    "Lastly, let's simulate a continuous-time system, and differentiate the sampled simulation results. We consider the linear system\n",
    "$$ \\dot{x} = -x,~~ y = x.$$\n",
    "Given the initial state $x_0$, its output solution is given as\n",
    "$$ y(t) = e^{-t} x_0,$$\n",
    "and the output's derivative w.r.t. the initial state is given as \n",
    "$$ \\frac{\\partial y}{\\partial x_0} = e^{-t},$$\n",
    "which only depends on time. Now we will calculate this derivative through automatic differentiation, and compare the results to the analytical gradients:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ccb8a40a-1193-4478-861c-da0bd3336d80",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "def calculate_analytical_gradient(t):\n",
    "    return np.exp(-t)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "36bf398d-2bdc-4410-9143-5ae45ec4fe68",
   "metadata": {},
   "source": [
    "Let's construct the linear system"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2190cd18-f2d5-49a1-8c2d-184336b91f37",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydrake.systems.primitives import LinearSystem, LogVectorOutput\n",
    "from pydrake.systems.framework import DiagramBuilder\n",
    "from pydrake.autodiffutils import InitializeAutoDiff, ExtractGradient, AutoDiffXd\n",
    "from pydrake.systems.analysis import Simulator_\n",
    "\n",
    "A = np.array([[-1]])\n",
    "B = np.array([[0]])\n",
    "C = np.array([[1]])\n",
    "D = np.array([[0]])\n",
    "timestep = 0  # so that the system is continuous-time\n",
    "linear_system = LinearSystem(A, B, C, D, timestep)\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "builder.AddSystem(linear_system)\n",
    "builder.ExportInput(linear_system.get_input_port(), \"input\")\n",
    "builder.ExportOutput(linear_system.get_output_port(), \"output\")\n",
    "logger = LogVectorOutput(linear_system.get_output_port(), builder, publish_period=0.1)\n",
    "system = builder.Build()  # The dynamical system we are considering"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b0caefe1-4909-4bf4-aef9-d844a1cd6ee4",
   "metadata": {},
   "source": [
    "and convert it to use `AutoDiffXd` scalar"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "998e4950-dba3-406f-a233-eb1e1a7b526c",
   "metadata": {},
   "outputs": [],
   "source": [
    "system_ad = system.ToAutoDiffXd()\n",
    "logger_ad = system_ad.GetSystems()[1]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "54f439ed-c911-4ed6-b7a2-33cd7c03e180",
   "metadata": {},
   "source": [
    "We get the `AutoDiffXd` version of the logger to extract the simulation results later. Now we construct a `Simulator` that uses `AutoDiffXd` scalar, and set an arbitrary initial state $x_0 = 5$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d6dee0e8-ed7c-4f39-9714-5ca5c891acbc",
   "metadata": {},
   "outputs": [],
   "source": [
    "simulator_ad = Simulator_[AutoDiffXd](system_ad)\n",
    "context_ad = simulator_ad.get_mutable_context()\n",
    "system_ad.get_input_port(0).FixValue(context_ad, 0)\n",
    "x0 = np.array([5])\n",
    "x0 = InitializeAutoDiff(x0)\n",
    "context_ad.SetContinuousState(x0)\n",
    "simulator_ad.Initialize()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "eceb1f13-609d-4426-b24c-048c8afdf448",
   "metadata": {},
   "source": [
    "Finally, we simulate for 1 second, and assert that the derivatives calculated by automatic differentiation match the analytical ones:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2ab0ec5b-7719-426f-91f4-d4d856674dfc",
   "metadata": {},
   "outputs": [],
   "source": [
    "simulator_ad.AdvanceTo(1)\n",
    "log = logger_ad.FindLog(simulator_ad.get_context())\n",
    "# convert AutoDiffXd back to double\n",
    "sample_times = np.array([t.value() for t in log.sample_times()])\n",
    "# calcualte gradients analytically and via autodiff\n",
    "analytical_gradients = calculate_analytical_gradient(sample_times)\n",
    "autodiff_gradients = ExtractGradient(log.data()).flatten()\n",
    "# Let's print the data at an arbitrary sample time\n",
    "sample_index = 3\n",
    "print(\"dy/dx0 at t =\", sample_times[sample_index]),\n",
    "print(\"Analytical gradient:\", analytical_gradients[sample_index])\n",
    "print(\"Gradient calculated by autodiff:\", autodiff_gradients[sample_index])\n",
    "# We assert that the autodiff gradients are correct at all sample times\n",
    "np.testing.assert_allclose(autodiff_gradients, analytical_gradients, atol=1e-6)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "67226a6f-084c-4d8a-9e0d-59370892bc13",
   "metadata": {},
   "source": [
    "## Further reading\n",
    "\n",
    "**System Scalar Types and Conversions in Drake**  \n",
    "- [Default Scalars](https://drake.mit.edu/doxygen_cxx/group__default__scalars.html): Overview of the scalar types commonly used in Drake, such as `double`, `AutoDiffXd`, and `symbolic::Expression`.  \n",
    "- [System Scalar Conversion](https://drake.mit.edu/doxygen_cxx/group__system__scalar__conversion.html): Describes how Drake systems support conversions between scalar types to enable features like automatic differentiation and symbolic analysis.\n",
    "\n",
    "**Automatic Differentiation with Eigen**  \n",
    "- [An Introduction to Automatic Differentiation in Eigen (PDF)](https://github.com/edrumwri/drake/blob/bbc944fec87f7dac13169c65c961db29906435fb/drake/doc/autodiff_intro/autodiff.pdf)\n",
    "\n",
    "**Automatic Differentiation with Drake’s Hydroelastic Contact Model**  \n",
    "- *Kurtz, V., & Lin, H.* (2022). Contact-Implicit Trajectory Optimization with Hydroelastic Contact and iLQR. *IEEE/RSJ IROS 2022*. [link](https://ieeexplore.ieee.org/abstract/document/9981686) [code](https://github.com/vincekurtz/drake_ddp)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bc71334d-4a59-4a9f-9cdd-acaf8131789e",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
